/**
 * @file chassis.c
 * @author NeoZeng neozng1@hnu.edu.cn
 * @brief 底盘应用,负责接收robot_cmd的控制命令并根据命令进行运动学解算,得到输出
 *        注意底盘采取右手系,对于平面视图,底盘纵向运动的正前方为y正方向;横向运动的左侧为x正方向
 *
 * @version 0.1
 * @date 2022-12-04
 *
 * @copyright Copyright (c) 2022
 *
 */

#include "chassis.h"

/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
#define L_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X) /1000.f)//转换为m
#define R_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X) /1000.f)//转换为m
/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
#include "can_comm.h"
#include "ins_task.h"
static CANCommInstance *chasiss_can_comm; // 双板通信CAN comm
attitude_t *Chassis_IMU_data;
#endif // CHASSIS_BOARD
#ifdef ONE_BOARD
static Publisher_t *chassis_pub;                    // 用于发布底盘的数据
static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控制命令
#endif                                              // !ONE_BOARD
static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
static const float Wheel_To_Core_Distance[2]={L_CENTER,R_CENTER};
static const float Wheel_Azimuth[2] = {PI, 0.0f};   // 舵轮轮组与y轴正方向夹角
static referee_info_t* referee_data; // 用于获取裁判系统的数据
static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据传入此结构体的对应变量中，UI会自动检测是否变化，对应显示UI

static SuperCapInstance *cap;                                       // 超级电容
static Motor_Wheel_s *motor_wheel[2];
static Motor_Steer_s *motor_steer[2];
static LightGateInstance *light_gate[2];
static Power_Sampling_Instance *power_sampling;
static float dt = 0;
/* 用于自旋变速策略的时间变量 */
static float t=0.3;
static const float find_zero_speed=10;
static Chassis_Velocity_s chassis_velocity;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */

static void Steer_Motor_Kinematics_Nearest_Transposition();

//对于舵轮，我们希望在初始化阶段让舵向电机向前，并将此时设为0度
void ChassisInit()
{
    light_gate[left] = LightGateInit(light_gate_l_Pin);
    light_gate[right] = LightGateInit(light_gate_r_Pin);
    Power_Sampling_Init_Config_s power_sampling_config = {
        .can_init_config.can_handle = &hcan1,
        .can_init_config.tx_id = POWER_REPORT_ID,
        .can_init_config.rx_id = POWER_REPORT_ID, 
    };
    power_sampling = PowerSamplingInit(&power_sampling_config);

    Motor_Init_Config_s motor_steer_config = {
        .can_init_config.can_handle = &hcan1,
        
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.5, // 0.4
                .Ki = 0.1, // 0
                .Kd = 0,
                .IntegralLimit = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 16,384,
            },
            .speed_PID = {
                .Kp = 8, // 4.5
                .Ki = 5,  // 0
                .Kd = 0,  // 0
                .IntegralLimit = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 16384,
            },
            .current_PID = {
                .Kp = 0.5, // 0.4
                .Ki = 0.1,   // 0
                .Kd = 0,
                .IntegralLimit = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 16384,
            },
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP | CURRENT_LOOP,
            
        },
        .motor_type = M3508,
    };
    Motor_Init_Config_s motor_wheel_config = {
        .can_init_config.can_handle = &hcan1,
        .controller_param_init_config = {
            .speed_PID = {
                .Kp = 8, // 4.5
                .Ki = 5, // 0
                .Kd = 0, // 0
                .IntegralLimit = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 16384,
            },
            .current_PID = {
                .Kp = 0.5, // 0.4
                .Ki = 0.1, // 0
                .Kd = 0,
                .IntegralLimit = 45000,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .MaxOut = 16384,
            },
        },
        .controller_setting_init_config = {
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = SPEED_LOOP,
            .close_loop_type = SPEED_LOOP | CURRENT_LOOP,
        },
        .motor_type = LK7015,
    };
    for (int i = 0; i < 2; i++)
    {
        motor_steer[i] = (Motor_Steer_s *)malloc(sizeof(Motor_Steer_s));
        motor_steer_config.can_init_config.tx_id = i + 1;
        motor_steer_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
        motor_steer[i]->motor = DJIMotorInit(&motor_steer_config);
        motor_wheel[i] = (Motor_Wheel_s *)malloc(sizeof(Motor_Wheel_s));
        motor_wheel_config.can_init_config.tx_id = i + 1;
        motor_wheel_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
        motor_wheel[i]->motor = LKMotorInit(&motor_wheel_config);
    }
    //  @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
    
    // referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI

    SuperCap_Init_Config_s cap_conf = {
        .can_config = {
            .can_handle = &hcan2,
            .tx_id = 0x302, // 超级电容默认接收id
            .rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
        }};
    cap = SuperCapInit(&cap_conf); // 超级电容初始化
    
    //这里需要读取光电门

    // 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
    Chassis_IMU_data = INS_Init(); // 底盘IMU初始化

    CANComm_Init_Config_s comm_conf = {
        .can_config = {
            .can_handle = &hcan2,
            .tx_id = 0x311,
            .rx_id = 0x312,
        },
        .recv_data_len = sizeof(Chassis_Ctrl_Cmd_s),
        .send_data_len = sizeof(Chassis_Upload_Data_s),
    };
    chasiss_can_comm = CANCommInit(&comm_conf); // can comm初始化
#endif                                          // CHASSIS_BOARD
#ifdef ONE_BOARD // 单板控制整车,则通过pubsub来传递消息
    chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
    chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
#endif // ONE_BOARD
}

static void SteerFindZero()
{
    //假设舵一开始顺时针旋转，若不是，修改即可
    if(LightGateUpdate(light_gate[left])==0||LightGateUpdate(light_gate[right])==0)
    {
        int i=0;
        if(LightGateUpdate(light_gate[i])==0)
        {
            DJIMotorSetRef(motor_steer[i]->motor, find_zero_speed);
        }
        else if(LightGateUpdate(light_gate[i])==1)//遮挡到未遮挡
        {
            motor_steer[i]->zero_angle=motor_steer[i]->motor->measure.angle_single_round;
            motor_steer[i]->motor->motor_settings.outer_loop_type=ANGLE_LOOP;
            DJIMotorSetRef(motor_steer[i]->motor,motor_steer[i]->zero_angle);
            motor_steer[i]->find_zero_flag=1;
        }
        else
        {
            motor_steer[i]->zero_angle=motor_steer[i]->motor->measure.angle_single_round;
            motor_steer[i]->motor->motor_settings.outer_loop_type=ANGLE_LOOP;
            DJIMotorSetRef(motor_steer[i]->motor,motor_steer[i]->zero_angle);
            motor_steer[i]->find_zero_flag=1;
            motor_wheel[i]->motor->motor_settings.motor_reverse_flag=MOTOR_DIRECTION_REVERSE;
        }
        i++;
        if(LightGateUpdate(light_gate[i])==0)
        {
            DJIMotorSetRef(motor_steer[i]->motor, find_zero_speed);
        }
        else if(LightGateUpdate(light_gate[i])==1)//遮挡到未遮挡
        {
            motor_steer[i]->zero_angle=motor_steer[i]->motor->measure.angle_single_round;
            motor_steer[i]->motor->motor_settings.outer_loop_type=ANGLE_LOOP;
            DJIMotorSetRef(motor_steer[i]->motor,motor_steer[i]->zero_angle);
            motor_steer[i]->find_zero_flag=1;
        }
        else
        {
            motor_steer[i]->zero_angle=motor_steer[i]->motor->measure.angle_single_round;
            motor_steer[i]->motor->motor_settings.outer_loop_type=ANGLE_LOOP;
            DJIMotorSetRef(motor_steer[i]->motor,motor_steer[i]->zero_angle);
            motor_steer[i]->find_zero_flag=1;
            motor_wheel[i]->motor->motor_settings.motor_reverse_flag=MOTOR_DIRECTION_REVERSE;
        }
    }
}





/**
 * @brief 计算每个电机的输出,底盘逆解,统一认为舵向电机逆时针旋转时角度增加
 *        用宏进行预替换减小开销,运动解算具体过程参考教程
 */
static void Kinematics_Inverse_Calculate()
{
    for(int i=0;i<2;i++)
    {
        //目标x,y，总，三个速度
        float tmp_velocity_x, tmp_velocity_y, tmp_velocity_modulus;
        //映射到单个轮组
        tmp_velocity_y=(chassis_velocity.Taget_chassis_vy + Wheel_To_Core_Distance[i] * chassis_velocity.Taget_chassis_wz*arm_cos_f32(Wheel_Azimuth[i]));
        tmp_velocity_x=chassis_velocity.Taget_chassis_vx;
        arm_sqrt_f32( tmp_velocity_x*tmp_velocity_x+tmp_velocity_y*tmp_velocity_y, &tmp_velocity_modulus);    
        motor_wheel[i]->Target_Wheel_Omega = tmp_velocity_modulus / RADIUS_WHEEL*1000.0f;//单位为国际单位制，rad/s
        if (tmp_velocity_modulus == 0.0f)
        {
            // 排除除零问题
            motor_steer[i]->Target_Steer_Angle = motor_steer[i]->motor->measure.angle_single_round;
        }
        else
        {
            // 没有除零问题
            motor_steer[i]->Target_Steer_Angle = atan2f(tmp_velocity_y, tmp_velocity_x);//目标舵向角度归一化到-PI到PI
        }
    }
    Steer_Motor_Kinematics_Nearest_Transposition();
   
}

/**
 * @brief 舵向电机依照轮向电机目标角速度就近转位
 *
 */
static void Steer_Motor_Kinematics_Nearest_Transposition()
{
    for(int i =0; i<2; i++)
    {
        float tmp_angle =Math_Modulus_Normalization(motor_steer[i]->motor->measure.angle_single_round-motor_steer[i]->zero_angle,2.0f*PI);//当前舵轮角度=编码器角度-零点角度
        float tmp_delta_angle = Math_Modulus_Normalization(motor_steer[i]->Target_Steer_Angle -tmp_angle ,2.0f*PI);//目标变化角度=目标角度-当前舵轮角度

        if(-PI / 2.0f <= tmp_delta_angle && tmp_delta_angle <= PI / 2.0f)
        {
            //不用反转,此时结果为最优
            motor_steer[i]->Target_Steer_Angle = tmp_delta_angle+motor_steer[i]->motor->measure.angle_single_round;
        }
        else
        {
            //要反转
            motor_steer[i]->Target_Steer_Angle = Math_Modulus_Normalization(tmp_delta_angle + PI, 2.0f * PI) +motor_steer[i]->motor->measure.angle_single_round;
            if(motor_wheel[i]->motor->motor_settings.motor_reverse_flag==MOTOR_DIRECTION_NORMAL)motor_wheel[i]->motor->motor_settings.motor_reverse_flag=MOTOR_DIRECTION_REVERSE;//轮向电机反转
            else motor_wheel[i]->motor->motor_settings.motor_reverse_flag=MOTOR_DIRECTION_NORMAL;
        }
    }
}
/**
 * @brief 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值，待添加
 * @todo 根据裁判系统和电容剩余容量对输出进行限制并设置电机参考值
 */
static void LimitChassisOutput()
{
    // 功率限制待添加
    // referee_data->PowerHeatData.chassis_power;
    // referee_data->PowerHeatData.chassis_power_buffer;

    // 完成功率限制，单位转换后进行电机参考输入设定
    for(int i=0; i<2; i++)
    {
        motor_steer[i]->Target_Steer_Angle=(motor_steer[i]->Target_Steer_Angle+motor_steer[i]->motor->measure.total_round*PI)*RAD_2_DEGREE;
        LKMotorSetRef(motor_wheel[i]->motor, motor_wheel[i]->Target_Wheel_Omega);
        DJIMotorSetRef(motor_steer[i]->motor, motor_steer[i]->Target_Steer_Angle);
    }
}

/**
 * @brief 根据每个轮子的速度反馈,计算底盘的实际运动速度,逆运动解算
 *        对于双板的情况,考虑增加来自底盘板IMU的数据
 *
 */
static void EstimateSpeed()
{
    //清零速度变量，开始正解
    chassis_velocity.Now_chassis_vx = 0;
    chassis_velocity.Now_chassis_vy = 0;
    chassis_velocity.Now_chassis_wz = 0;

    for(int i=0 ; i<2 ;i++)
    {
         float tmp_angle_modulus=motor_steer[i]->motor->measure.angle_single_round-motor_steer[i]->zero_angle;
        chassis_velocity.Now_chassis_vx +=motor_wheel[i]->motor->measure.speed_rads*RADIUS_WHEEL/1000.0f*arm_cos_f32(tmp_angle_modulus);
        
    }


}

/* 机器人底盘控制核心任务 */
void ChassisTask()
{
    // 后续增加没收到消息的处理(双板的情况)
    // 获取新的控制信息
#ifdef ONE_BOARD
    SubGetMessage(chassis_sub, &chassis_cmd_recv);
#endif
#ifdef CHASSIS_BOARD
    chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD

    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)//失能且电机找到零点
    { // 如果出现重要模块离线或遥控器设置为急停,让电机停止
        LKMotorStop(motor_wheel[left] -> motor);
        LKMotorStop(motor_wheel[right] ->motor);

        DJIMotorStop(motor_steer[left] ->motor);
        DJIMotorStop(motor_steer[right]->motor);
        ui_data.chassis_mode = CHASSIS_ZERO_FORCE;
    }
    else
    { // 正常工作
        LKMotorEnable(motor_wheel[left] -> motor);
        LKMotorEnable(motor_wheel[right]-> motor);
        DJIMotorEnable(motor_steer[left]-> motor);
        DJIMotorEnable(motor_steer[right]->motor);
    }
    if( motor_steer[left]->find_zero_flag==true &&motor_steer[right]->find_zero_flag==true)
    {
        DJIMotorEnable(motor_steer[left]-> motor);
        DJIMotorEnable(motor_steer[right]->motor);
        chassis_cmd_recv.chassis_mode=CHASSIS_FIND_ZERO;
    }
    // 根据控制模式设定旋转速度
    switch (chassis_cmd_recv.chassis_mode)
    {
    case CHASSIS_FIND_ZERO:// 寻找零点
          SteerFindZero();
        break;
    case CHASSIS_NO_FOLLOW: // 底盘不旋转,但维持全向机动,一般用于调整云台姿态
        chassis_cmd_recv.wz = 0;
        ui_data.chassis_mode = CHASSIS_NO_FOLLOW;
        break;
    case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
        chassis_velocity.Taget_chassis_wz = 5.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
        if(chassis_velocity.Taget_chassis_wz>=2500)chassis_velocity.Taget_chassis_wz=2500;
        if(chassis_velocity.Taget_chassis_wz<=-2500)chassis_velocity.Taget_chassis_wz=-2500;
        ui_data.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
        // wz=0;
        break;
    case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
        // int get_wz =get();
        chassis_velocity.Taget_chassis_wz = 3500;
        ui_data.chassis_mode = CHASSIS_ROTATE;
        break;
    default:
        break;
    }

    // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
    // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
    static float sin_theta, cos_theta;
    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
    chassis_velocity.Taget_chassis_vx = +chassis_cmd_recv.vx * cos_theta + chassis_cmd_recv.vy * sin_theta;
    chassis_velocity.Taget_chassis_vy  = -chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
    //计算底盘输出
    Kinematics_Inverse_Calculate();

    // 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
    LimitChassisOutput();
    // PowerSamplingTask(power_sampling);
    // 根据电机的反馈速度和IMU(如果有)计算真实速度
    EstimateSpeed();

    // // 获取裁判系统数据   建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
    // // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
    // chassis_feedback_data.enemy_color = referee_data->GameRobotState.robot_id > 7 ? 1 : 0;
    // // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
    // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
    // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

    // 推送反馈消息
#ifdef ONE_BOARD
    PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
#endif
#ifdef CHASSIS_BOARD
    CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
#endif // CHASSIS_BOARD
}
